import traci
import traci.constants as tc

from scene.constants import OFF, ON, DEL, SPEED_LIMIT_POLYGON_SIGN
from utils.common_util import calculate_acceleration, calculate_distance, calc_time_to_reach
from utils.lon_lat_util import latAndLonConversionXY


class SpeedLimitDomain:

    def __init__(self):
        self.speed_limit_polygon = {}
        self.distance_dicts = {}
        self.sumo_net_instance = None
    def set_new_speed_limit_area(self, sumo_net_instance: None, polygon_id: str, area_points: list):
        """
            设置限速的感知区域
        """
        self.speed_limit_polygon[polygon_id] = area_points
        self.sumo_net_instance = sumo_net_instance
    def get_speed_limit_polygon(self, polygon_id: str):
        """
            获取指定限速区域
        """
        return self.speed_limit_polygon.get(polygon_id)

    def get_speed_limit_polygon_list(self):
        """
            获取所有限速区域
        """
        if len(self.speed_limit_polygon) < 1:
            return []
        return list(map(lambda item: item[1].to_dict(), self.speed_limit_polygon.items()))

    def control_speed_limit(self, polygon_id: str, switch: int):
        """
            控制限速区域
            param: polygon_id: 区域id
            param: switch: -1：删除 1：关闭 0：开启
            return: 0: 失败 1：成功
        """
        assert polygon_id in self.speed_limit_polygon.keys(), '限速区域不存在'
        result = 0
        speed_limit_item = self.speed_limit_polygon[polygon_id]
        if speed_limit_item is not None and speed_limit_item.switch != switch:
            area_pos = list()
            if switch == ON:
                for point in speed_limit_item:
                    area_pos.append(latAndLonConversionXY(self.sumo_net_instance, point))
                traci.polygon.add(SPEED_LIMIT_POLYGON_SIGN, area_pos, (1, 0, 1))
                traci.polygon.subscribeContext(SPEED_LIMIT_POLYGON_SIGN, tc.CMD_GET_VEHICLE_VARIABLE, 20,
                                               [tc.VAR_SPEED, tc.VAR_DECEL, tc.VAR_DISTANCE, tc.VAR_TYPE,
                                                tc.VAR_POSITION])
            else:
                traci.polygon.remove(SPEED_LIMIT_POLYGON_SIGN)
            result = 1
        return result

    def control_vehicles(self, vehicles: dict, speed_limit_points: list, speed_speeds: list):
        """
            控制车辆
            param: vehicles: 车辆列表
            param: speed_limit_points: 限速区域点
            param: speeds: 限速区域速度
        """
        assert len(speed_limit_points) == len(speed_speeds), '限速区域点和速度不匹配'
        array = []
        try:
            for vehicle_id, vehicle_info in vehicles.items():
                array.append(vehicle_id)
                # 如果车辆此时没在这个区域内，则可能是刚刚进入这个区域，也可能已经处于这个区域内
                if vehicle_id not in self.distance_dicts:
                    veh_distance = 99999999
                    number = 0
                    # 此时判断离该车辆最近的经纬度点
                    for i in range(len(speed_limit_points)):
                        lat = float(speed_limit_points[i]["lat"])
                        lon = float(speed_limit_points[i]["lon"])
                        veh_pos = vehicle_info[tc.VAR_POSITION]
                        veh_lon, veh_lat = traci.simulation.convertGeo(veh_pos[0], veh_pos[1])
                        veh_distance2 = calculate_distance(veh_lon, veh_lat, lon, lat)
                        if veh_distance2 <= veh_distance:
                            number = i
                            veh_distance = veh_distance2
                            self.distance_dicts[vehicle_id] = [lat, lon, speed_speeds[i]]
                    # 计算车辆的加速度
                    acceleration = calculate_acceleration(float(vehicle_info[tc.VAR_SPEED]),
                                                          float(speed_speeds[number]) / 3.6, veh_distance)
                    t = calc_time_to_reach(float(speed_speeds[number]) / 3.6, str(veh_distance),
                                            vehicle_info[tc.VAR_SPEED], acceleration)
                    traci.vehicle.slowDown(vehicle_id, float(speed_speeds[number]) / 3.6, t)
                    # traci.vehicle.setSpeed(vehicle_id, float(HttpInterface.speeds[number])/3.6)
                else:
                    # 首先判断车辆是否已经经过了这个点位（误差距离5m）
                    points = self.distance_dicts[vehicle_id]
                    veh_pos = vehicle_info[tc.VAR_POSITION]
                    veh_lon, veh_lat = traci.simulation.convertGeo(veh_pos[0], veh_pos[1])
                    distance = calculate_distance(veh_lon, veh_lat, points[1], points[0])
                    # 通过判断车辆车速 对未生效的车辆速度进行校正
                    if vehicle_info[tc.VAR_SPEED] > float(points[2]) / 3.6:
                        acceleration = calculate_acceleration(float(vehicle_info[tc.VAR_SPEED]),
                                                              float(points[2]) / 3.6, distance)
                        t = calc_time_to_reach(float(points[2]) / 3.6, str(distance),
                                                vehicle_info[tc.VAR_SPEED], acceleration)
                        traci.vehicle.slowDown(vehicle_id, float(points[2]) / 3.6, t)
                        # traci.vehicle.setSpeed(vehicle_id, float(points[2]) / 3.6)
                    if distance < 10:
                        # 说明车辆已经经过了这个点位，则需要判断下一个点位
                        number = 0
                        for i in range(len(speed_limit_points)):
                            lat = float(speed_limit_points[i]["lat"])
                            lon = float(speed_limit_points[i]["lon"])
                            if lat == points[0] and lon == points[1]:
                                number = i
                                break
                        # print(vehicles.items())
                        if number == len(speed_limit_points) - 1:
                            continue
                        else:
                            # 说明车辆还没有经过最后一个点位，则需要判断下一个点位
                            lat = float(speed_limit_points[number + 1]["lat"])
                            lon = float(speed_limit_points[number + 1]["lon"])
                            self.distance_dicts[vehicle_id] = [lat, lon, speed_speeds[number + 1]]
                            veh_pos = vehicle_info[tc.VAR_POSITION]
                            veh_lon, veh_lat = traci.simulation.convertGeo(veh_pos[0], veh_pos[1])
                            distance = calculate_distance(veh_lon, veh_lat, lon, lat)
                            acceleration = calculate_acceleration(float(vehicle_info[tc.VAR_SPEED]),
                                                                  float(speed_speeds[number + 1]) / 3.6, distance)
                            t = calc_time_to_reach(float(speed_speeds[number + 1]) / 3.6, str(distance),
                                                    vehicle_info[tc.VAR_SPEED], acceleration)
                            traci.vehicle.slowDown(vehicle_id, float(speed_speeds[number + 1]) / 3.6, t)
                            # traci.vehicle.setSpeed(vehicle_id, float(HttpInterface.speeds[number+1]) / 3.6)
            # 获取数据在array里面，而不在distance_dicts的keys中的数据
            resultList = [a for a in self.distance_dicts.keys() if a not in array]
            for i in resultList:
                self.distance_dicts.pop(i)
                traci.vehicle.setSpeed(i, -1)
        except Exception as e:
            print(e)

class SpeedLimitItem:

    def __new__(cls, *args, **kwargs):
        return super().__new__(cls)